/*
 Lab 2WD  -  two wheel with L298N
 Motor Red  +  High ->  Forward
 Motor Black -  High -> Back

*/

#ifndef MOTOR_L298N_H
#define MOTOR_L298N_H
                                             
// 正反转控制
#define RightBack 9    // Motor balck -  LS298N IN1-> Arduino Pin9
#define RightForward 8 // Motor red +   LS298N IN2 -> Arduino Pin8

#define LeftForward 5 // Motor  black - LS298N IN3 -> Arduino Pin5
#define LeftBack 4    // Motor Red +   LS298N IN4 -> Arduino Pin4

// 速度控制
#define leftPWM 10
#define rightPWM 6

// car action code
const int CAR_FORWARD = 1;
const int CAR_BACKWARD = 2;
const int CAR_LEFT = 3;
const int CAR_RIGHT = 4;
const int CAR_STOP = 5;
//
const int CAR_SPEED_UP = 6;
const int CAR_SPEED_DOWN = 7;

// car running state
int cur_car_cmd = CAR_STOP;
int car_state = CAR_STOP;

const int speed_level_min = 180; // 太低驱动不了
const int speed_level_max = 250;
// 左右轮子由于物理差异，同样的PWM下，实际速度不一样
// 需要分别给PWM数值，调整的实际速度一样
//   the_diff_right_left  数值需要测试得到
// 此外，2wd的万向轮位置影响直线前进，如果要直线前进，需要摆正万向轮位置
const int the_diff_right_left=10;
int cur_left_speed_level;
int cur_right_speed_level; 

void car_init()
{
  digitalWrite(LeftForward, LOW);
  digitalWrite(LeftBack, LOW);
  digitalWrite(RightForward, LOW);
  digitalWrite(RightBack, LOW);
}

void car_forward()
{
  // 左轮前进
  digitalWrite(LeftForward, HIGH);
  digitalWrite(LeftBack, LOW);
  // 右轮前进
  digitalWrite(RightForward, HIGH);
  digitalWrite(RightBack, LOW);
}

void car_backward()
{
  // 左轮后退
  digitalWrite(LeftForward, LOW);
  digitalWrite(LeftBack, HIGH);
  // 右轮后退
  digitalWrite(RightForward, LOW);
  digitalWrite(RightBack, HIGH);
}

/* 左转 */
void car_turn_left()
{
  // 左轮不动
  digitalWrite(LeftForward, LOW);
  digitalWrite(LeftBack, LOW);
  // 右轮前进
  digitalWrite(RightForward, HIGH);
  digitalWrite(RightBack, LOW);
}

/* 右转 */
void car_turn_right()
{
  // 左轮前进
  digitalWrite(LeftForward, HIGH);
  digitalWrite(LeftBack, LOW);
  // 右轮不动
  digitalWrite(RightForward, LOW);
  digitalWrite(RightBack, LOW);
}

void car_stop()
{
  digitalWrite(LeftForward, LOW);
  digitalWrite(LeftBack, LOW);
  digitalWrite(RightForward, LOW);
  digitalWrite(RightBack, LOW);
}

void car_speed_up()
{
  cur_left_speed_level += 20;
  cur_right_speed_level += 20;
  
  if (cur_left_speed_level > speed_level_max)
  {
    cur_left_speed_level = speed_level_max;
  };
  if (cur_right_speed_level > speed_level_max)
  {
    cur_right_speed_level = speed_level_max;
  };
  analogWrite(leftPWM, cur_left_speed_level);
  analogWrite(rightPWM, cur_right_speed_level);
}

void car_speed_down()
{
  cur_left_speed_level -= 20;
  cur_right_speed_level -= 20;
  if (cur_left_speed_level < speed_level_min)
  {
    cur_left_speed_level = speed_level_min;
  };
  if (cur_right_speed_level < speed_level_min)
  {
    cur_right_speed_level = speed_level_min;
  };
  analogWrite(leftPWM, cur_left_speed_level);
  analogWrite(rightPWM, cur_right_speed_level);
}

void car_action(int car_cmd)
{
  switch (car_cmd)
  {
  case CAR_FORWARD:
    car_forward();
    car_state = CAR_FORWARD;
    break;
  case CAR_LEFT:
    car_turn_left();
    car_state = CAR_LEFT;
    break;
  case CAR_RIGHT:
    car_turn_right();
    car_state = CAR_RIGHT;
    break;
  case CAR_BACKWARD:
    car_backward();
    car_state = CAR_BACKWARD;
    break;
  case CAR_STOP:
    car_stop();
    car_state = CAR_STOP;
    break;
  case CAR_SPEED_UP:
    car_speed_up();
    break;
  case CAR_SPEED_DOWN:
    car_speed_down();
    break;
  default:
    break;
  } // switch

};

void setup_l298n()
{
  pinMode(LeftForward, OUTPUT);
  pinMode(LeftBack, OUTPUT);
  pinMode(RightForward, OUTPUT);
  pinMode(RightBack, OUTPUT);

  car_init();

  // speed control
  pinMode(leftPWM, OUTPUT);
  pinMode(rightPWM, OUTPUT);

  cur_left_speed_level = speed_level_min;
  cur_right_speed_level=cur_left_speed_level+the_diff_right_left;
  analogWrite(leftPWM, cur_left_speed_level);
  analogWrite(rightPWM, cur_right_speed_level);
}

void loop_l298n()
{
}

#endif /* MOTOR_L298N_H */